
#include "shqueue.h"
#include <errno.h>
#include <stdio.h>


#define uintmax_t	unsigned long long
#define size_t int
#define u_long int
#define u_int32_t unsigned int
void printx(char * addr, int len);



typedef struct __alloc_layout {
	SH_TAILQ_HEAD(__addrq) addrq;		/* Sorted by address */
	SH_TAILQ_HEAD(__sizeq) sizeq;		/* Sorted by size */

	u_int32_t success;			/* Successful allocations */
	u_int32_t failure;			/* Failed allocations */
	u_int32_t freed;			/* Free calls */
	u_int32_t longest;			/* Longest chain walked */
	uintmax_t  unused;			/* Guarantee alignment */
} ALLOC_LAYOUT;

typedef struct __alloc_element {
	SH_TAILQ_ENTRY addrq;			/* List by address */
	SH_TAILQ_ENTRY sizeq;			/* List by size */

	/*
	 * The "len" field is the total length of the chunk, not the size
	 * available to the caller.
	 */
	size_t len;				/* Chunk length */

	/*
	 * The "ulen" field is the length returned to the caller.
	 *
	 * Set to 0 if the chunk is not currently in use.
	 */
	uintmax_t ulen;				/* User's length */
} ALLOC_ELEMENT;

/*
 * __env_alloc_init --
 *	Initialize the area as one large chunk.
 *
 * PUBLIC: void __env_alloc_init __P((REGINFO *, size_t));
 */
void
__env_alloc_init(void *in, size_t size)
{
	ALLOC_ELEMENT *elp;
	ALLOC_LAYOUT *head;


	/*
	 * The first chunk of memory is the ALLOC_LAYOUT structure.
	 */
	head = in;
	SH_TAILQ_INIT(&head->addrq);
	SH_TAILQ_INIT(&head->sizeq);
	((head->success = head->failure = head->freed = 0));
	(head->unused=0);


	/*
	 * The rest of the memory is the first available chunk.
	 */
	elp = (ALLOC_ELEMENT *)((u_int8_t *)head + sizeof(ALLOC_LAYOUT));
	elp->len = size - sizeof(ALLOC_LAYOUT);
	elp->ulen = 0;

	SH_TAILQ_INSERT_HEAD(&head->addrq, elp, addrq, __alloc_element);
	SH_TAILQ_INSERT_HEAD(&head->sizeq, elp, sizeq, __alloc_element);
}

/*
 * The length, the ALLOC_ELEMENT structure and an optional guard byte,
 * rounded up to standard alignment.
 */

#define DB_ALIGN(v, bound)                      \
	    (((v) + (bound) - 1) & ~(((uintmax_t)(bound)) - 1))
#define	DB_ALLOC_SIZE(len)						\
	(size_t)DB_ALIGN((len) + sizeof(ALLOC_ELEMENT), sizeof(uintmax_t))

/*
 * __env_alloc_overhead --
 *	Return the overhead needed for an allocation.
 *
 * PUBLIC: size_t __env_alloc_overhead __P((void));
 */
size_t
__env_alloc_overhead()
{
	return (sizeof(ALLOC_ELEMENT));
}

/*
 * __env_alloc_size --
 *	Return the space needed for an allocation, including alignment.
 *
 * PUBLIC: size_t __env_alloc_size __P((size_t));
 */
size_t
__env_alloc_size(len)
	size_t len;
{
	return (DB_ALLOC_SIZE(len));
}

/*
 * __env_alloc --
 *	Allocate space from the shared region.
 *
 * PUBLIC: int __env_alloc __P((REGINFO *, size_t, void *));
 */
int
__env_alloc(void *infop, size_t len, void *retp)
{
	ALLOC_ELEMENT *elp, *frag, *elp_tmp;
	ALLOC_LAYOUT *head;
	size_t total_len;
	u_int8_t *p;
	u_long st_search;


	*(void **)retp = NULL;

	head = infop;

	total_len = DB_ALLOC_SIZE(len);
	printf("total_len=%d\n",total_len);

	/*
	 * If the chunk can be split into two pieces, with the fragment holding
	 * at least 64 bytes of memory, we divide the chunk into two parts.
	 */
#define	SHALLOC_FRAGMENT	(sizeof(ALLOC_ELEMENT) + 64)

	/*
	 * Walk the size queue, looking for the smallest slot that satisfies
	 * the request.
	 */
	elp = NULL;
	((st_search = 0));
	SH_TAILQ_FOREACH(elp_tmp, &head->sizeq, sizeq, __alloc_element) {
		((++st_search));
		if (elp_tmp->len < total_len)
			break;
		elp = elp_tmp;
		/*
		 * We might have a long list of chunks of the same size.  Stop
		 * looking if we won't fragment memory by picking the current
		 * slot.
		 */
		if (elp->len - total_len <= SHALLOC_FRAGMENT)
			break;
	}
	if (head->longest < st_search)
		head->longest = st_search;

	/*
	 * If we don't find an element of the right size, we're done.
	 */
	if (elp == NULL) {
		((++head->failure));
		return (ENOMEM);
	}
	((++head->success));

	/* Pull the chunk off of the size queue. */
	SH_TAILQ_REMOVE(&head->sizeq, elp, sizeq, __alloc_element);

	if (elp->len - total_len > SHALLOC_FRAGMENT) {
		frag = (ALLOC_ELEMENT *)((u_int8_t *)elp + total_len);
		frag->len = elp->len - total_len;
		frag->ulen = 0;

		elp->len = total_len;

		/* The fragment follows the chunk on the address queue. */
		SH_TAILQ_INSERT_AFTER(
		    &head->addrq, elp, frag, addrq, __alloc_element);

		/*
		 * Insert the fragment into the appropriate place in the
		 * size queue.
		 */
		SH_TAILQ_FOREACH(elp_tmp, &head->sizeq, sizeq, __alloc_element)
			if (elp_tmp->len < frag->len)
				break;
		if (elp_tmp == NULL)
			SH_TAILQ_INSERT_TAIL(&head->sizeq, frag, sizeq);
		else
			SH_TAILQ_INSERT_BEFORE(&head->sizeq,
			    elp_tmp, frag, sizeq, __alloc_element);
	}

	p = (u_int8_t *)elp + sizeof(ALLOC_ELEMENT);
	elp->ulen = len;
	*(void **)retp = p;

	return (0);
}

int main()
{
	char input[512]={0};
	printf("input=%p\n",input);
	__env_alloc_init(input,sizeof(input));
	printx(input,sizeof(input));
	char *p;
	__env_alloc(input,14,&p);
	printf("p1=%p\n",p);
	printx(input,sizeof(input));
	__env_alloc(input,7,&p);
	printf("p2=%p\n",p);
	printx(input,sizeof(input));
	__env_alloc(input,18,&p);
	printf("p3=%p\n",p);
	printx(input,sizeof(input));

	return 0;
}

void printx(char * addr, int len)
{
	int i;
	printf("\t");
	for(i=0;i<8;i++)
		printf("%d        ",i);
	printf("\n");
	for(i=0;i<len;i++){
		if(0 == i%4)
			printf(" ");
		if(0 == i%32)
			printf("\n%04d:\t",i);
		printf("%02x",(unsigned char)addr[i]);
	}
	printf("\n");
}

